using UnityEngine;
using Unity.MLAgents;
using Unity.MLAgents.Actuators;
using Unity.MLAgents.Sensors;
using Random = UnityEngine.Random;
using System.Collections.Generic;

public class TinkerSoccer : Agent
{
    int tp = 0;
    int tp0 = 0;

    float uf1 = 0f;
    float uf2 = 0f;
    readonly float[] u = new float[12];
    readonly float[] utotal = new float[12];

    Transform body;

    public Rigidbody ball;
    public bool fixbody = false;
    public bool train;
    public bool accelerate;
    public bool wasd = false;
    public int ObservationNum;
    public int ActionNum;

    readonly List<float> P0 = new List<float>();
    readonly List<float> W0 = new List<float>();
    Vector3 pos0;
    Vector3 posb0;
    Quaternion rot0;
    Quaternion rotb0;
    ArticulationBody[] arts = new ArticulationBody[40];
    readonly ArticulationBody[] acts = new ArticulationBody[12];

    public float vr = 0f;
    public float wr = 0f;
    float currentWr = 0f;
    float currentVr = 0f;

    [Header("Soccer References")]
    public Transform opponent;
    public Transform opponentGoal;
    public Transform homeGoal;

    [Header("Soccer Strategy Settings")]
    public float chaseSpeed = 0.9f;
    public float defendSpeed = 0.65f;
    public float retreatSpeed = 0.45f;
    public float headingGain = 28f;
    public float turnResponsiveness = 4.5f;
    public float forwardResponsiveness = 1.8f;
    public float possessionDistance = 0.6f;
    public float kickPreparationDistance = 0.75f;
    public float kickAlignmentTolerance = 12f;
    public float avoidanceRadius = 1.1f;
    public float avoidanceTurn = 0.6f;
    public float interceptBuffer = 1.2f;
    public float interceptVelocityScale = 0.35f;
    public float distanceBias = 0.05f;

    [Header("Alignment Settings")]
    public float ballAlignmentTolerance = 6f;
    public float goalAlignmentTolerance = 8f;
    public float alignmentTurnGain = 18f;
    public float goalTurnGain = 22f;
    public float closeApproachSpeed = 0.55f;

    [Header("Kick Settings")]
    public float kickDuration = 0.28f;
    public float kickCooldown = 0.9f;
    public float kickHipAmplitude = 40f;
    public float kickKneeAmplitude = 70f;
    public float kickAnkleAmplitude = 35f;

    [Header("Locomotion Tuning")]
    public float strideOffset = 38f;
    public float strideAmplitude = 22f;
    public int gaitHalfCycleFrames = 18;
    public float minTrainingSpeed = 0.45f;
    public float forwardSpeedRewardScale = 0.25f;
    public float lateralPenaltyScale = 0.07f;
    public int configuredMaxStep = 2800;
    public int stagnationStepLimit = 800;
    public float stagnationDistanceThreshold = 0.02f;

    [Header("Goal Detection")]
    public float goalDetectionRadius = 0.55f;

    [Header("Motion Control")]
    public float moveAfterTurnTolerance = 3f;

    [Header("Debug")]
    public bool enableDebugDraw = false;

    enum KickFoot { None, Left, Right }

    KickFoot activeKickFoot = KickFoot.None;

    float desiredVr = 0f;
    float desiredWr = 0f;
    float kickTimer = 0f;
    float kickCooldownTimer = 0f;
    float prevBallDistance = 0f;
    float prevBallGoalDistance = 0f;
    ArticulationBody opponentBody;
    bool _isClone = false;
    bool goalRegisteredThisEpisode = false;

    int stagnationStepCount = 0;
    Vector3 lastBallPosition = Vector3.zero;
    Vector3 lastBodyPosition = Vector3.zero;

    public override void Initialize()
    {
        arts = GetComponentsInChildren<ArticulationBody>();
        ActionNum = 0;
        for (int k = 0; k < arts.Length; k++)
        {
            if (arts[k].jointType == ArticulationJointType.RevoluteJoint && ActionNum < acts.Length)
            {
                acts[ActionNum] = arts[k];
                ActionNum++;
            }
        }

        body = arts[0].transform;
        pos0 = body.position;
        rot0 = body.rotation;
        if (ball != null)
        {
            posb0 = ball.position;
            rotb0 = ball.rotation;
            lastBallPosition = ball.position;
        }

        arts[0].GetJointPositions(P0);
        arts[0].GetJointVelocities(W0);
        accelerate = train;

        CacheOpponentBody();

        if (configuredMaxStep > 0)
        {
            MaxStep = configuredMaxStep;
        }

        lastBodyPosition = body.position;
    }

    void CacheOpponentBody()
    {
        if (opponent == null)
        {
            opponentBody = null;
            return;
        }

        opponentBody = opponent.GetComponentInChildren<ArticulationBody>();
        if (opponentBody == null)
        {
            opponentBody = opponent.GetComponent<ArticulationBody>();
        }
    }

    void Start()
    {
        Time.fixedDeltaTime = 0.01f;

        if (train && !_isClone)
        {
            for (int i = 1; i < 4; i++)
            {
                GameObject clone = Instantiate(gameObject);
                clone.name = $"{name}_Clone_{i}";
                var controller = clone.GetComponent<TinkerSoccer>();
                controller._isClone = true;
                controller.train = true;
                controller.accelerate = accelerate;
            }
        }
    }

    public override void OnEpisodeBegin()
    {
        tp = 0;
        kickTimer = 0f;
        kickCooldownTimer = 0f;
        activeKickFoot = KickFoot.None;
        stagnationStepCount = 0;
        goalRegisteredThisEpisode = false;

        for (int i = 0; i < u.Length; i++)
        {
            u[i] = 0f;
        }

        ObservationNum = 9 + 2 * ActionNum + 12;

        if (fixbody)
        {
            arts[0].immovable = true;
        }

        if (!fixbody)
        {
            arts[0].TeleportRoot(pos0, rot0);
            arts[0].velocity = Vector3.zero;
            arts[0].angularVelocity = Vector3.zero;
            arts[0].SetJointPositions(P0);
            arts[0].SetJointVelocities(W0);
        }

        ResetBallState();

        vr = 0f;
        wr = 0f;

        lastBodyPosition = body.position;

        if (train)
        {
            float minSpeed = Mathf.Min(chaseSpeed, minTrainingSpeed);
            float maxSpeed = Mathf.Max(chaseSpeed, minTrainingSpeed + 0.25f);
            vr = Random.Range(minSpeed, maxSpeed);
            wr = Random.Range(-1f, 1f);
        }
    }

    void ResetBallState()
    {
        if (ball == null)
        {
            prevBallDistance = 0f;
            prevBallGoalDistance = 0f;
            lastBallPosition = Vector3.zero;
            return;
        }

        ball.velocity = Vector3.zero;
        ball.angularVelocity = Vector3.zero;
        ball.position = posb0;
        ball.rotation = rotb0;

        prevBallDistance = Vector3.Distance(body.position, ball.position);
        prevBallGoalDistance = opponentGoal != null ? Vector3.Distance(ball.position, opponentGoal.position) : 0f;
        lastBallPosition = ball.position;
    }

    public override void CollectObservations(VectorSensor sensor)
    {
        sensor.AddObservation(body.InverseTransformDirection(Vector3.down));
        sensor.AddObservation(body.InverseTransformDirection(arts[0].angularVelocity));
        sensor.AddObservation(body.InverseTransformDirection(arts[0].velocity));

        for (int i = 0; i < ActionNum; i++)
        {
            sensor.AddObservation(acts[i].jointPosition[0]);
            sensor.AddObservation(acts[i].jointVelocity[0]);
        }

        Vector3 ballPosLocal = Vector3.zero;
        if (ball != null)
        {
            ballPosLocal = body.InverseTransformPoint(ball.position);
        }

        Vector3 opponentPosLocal = Vector3.zero;
        if (opponent != null)
        {
            opponentPosLocal = body.InverseTransformPoint(opponent.position);
        }

        Vector3 opponentGoalLocal = Vector3.zero;
        Vector3 homeGoalLocal = Vector3.zero;
        if (opponentGoal != null)
        {
            opponentGoalLocal = body.InverseTransformPoint(opponentGoal.position);
        }
        if (homeGoal != null)
        {
            homeGoalLocal = body.InverseTransformPoint(homeGoal.position);
        }

        sensor.AddObservation(ballPosLocal);
        sensor.AddObservation(opponentPosLocal);
        sensor.AddObservation(opponentGoalLocal);
        sensor.AddObservation(homeGoalLocal);

        sensor.AddObservation(vr);
        sensor.AddObservation(wr);
    }

    float EulerTrans(float eulerAngle)
    {
        return eulerAngle <= 180f ? eulerAngle : eulerAngle - 360f;
    }

    public override void OnActionReceived(ActionBuffers actionBuffers)
    {
        for (int i = 0; i < utotal.Length; i++)
        {
            utotal[i] = 0f;
        }

        var continuousActions = actionBuffers.ContinuousActions;
        const float kk = 0.9f;
        float[] kb = new float[12] { 12f, 12f, 34f, 56f, 34f, 12f, 12f, 34f, 56f, 34f, 0f, 0f };

        for (int i = 0; i < ActionNum; i++)
        {
            u[i] = u[i] * kk + (1f - kk) * continuousActions[i];
            utotal[i] = kb[i] * u[i];
            if (fixbody)
            {
                utotal[i] = 0f;
            }
        }

        float d0 = strideOffset;
        float dh = strideAmplitude;
        int[] idx = new int[6] { -2, -3, 4, 7, 8, -9 };

        if (ActionNum > 9)
        {
            utotal[Mathf.Abs(idx[0])] += (dh * uf1 + d0) * Mathf.Sign(idx[0]);
            utotal[Mathf.Abs(idx[1])] -= 2f * (dh * uf1 + d0) * Mathf.Sign(idx[1]);
            utotal[Mathf.Abs(idx[2])] += (dh * uf1 + d0) * Mathf.Sign(idx[2]);
            utotal[Mathf.Abs(idx[3])] += (dh * uf2 + d0) * Mathf.Sign(idx[3]);
            utotal[Mathf.Abs(idx[4])] -= 2f * (dh * uf2 + d0) * Mathf.Sign(idx[4]);
            utotal[Mathf.Abs(idx[5])] += (dh * uf2 + d0) * Mathf.Sign(idx[5]);
        }

        ApplyKickOffsets();

        for (int i = 0; i < ActionNum; i++)
        {
            SetJointTargetDeg(acts[i], utotal[i]);
        }
    }

    void ApplyKickOffsets()
    {
        if (kickTimer <= 0f || ActionNum < 10)
        {
            return;
        }

        float normalized = 1f - Mathf.Clamp01(kickTimer / Mathf.Max(0.001f, kickDuration));
        float swing = Mathf.Sin(normalized * Mathf.PI);

        int hipIndex = activeKickFoot == KickFoot.Left ? 2 : 7;
        int kneeIndex = activeKickFoot == KickFoot.Left ? 3 : 8;
        int ankleIndex = activeKickFoot == KickFoot.Left ? 4 : 9;

        if (hipIndex >= 0 && hipIndex < utotal.Length)
        {
            utotal[hipIndex] += kickHipAmplitude * swing;
        }
        if (kneeIndex >= 0 && kneeIndex < utotal.Length)
        {
            utotal[kneeIndex] -= kickKneeAmplitude * swing;
        }
        if (ankleIndex >= 0 && ankleIndex < utotal.Length)
        {
            utotal[ankleIndex] += kickAnkleAmplitude * swing;
        }

        kickTimer = Mathf.Max(0f, kickTimer - Time.fixedDeltaTime);
        if (kickTimer <= 0f)
        {
            activeKickFoot = KickFoot.None;
        }
    }

    void SetJointTargetDeg(ArticulationBody joint, float x)
    {
        var drive = joint.xDrive;
        drive.stiffness = 2200f;
        drive.damping = 110f;
        drive.forceLimit = 380f;
        drive.target = x;
        joint.xDrive = drive;
    }

    public override void Heuristic(in ActionBuffers actionsOut)
    {
    }

    void FixedUpdate()
    {
        if (opponentBody == null)
        {
            CacheOpponentBody();
        }

        bool autopilotActive = !train && opponent != null && opponentGoal != null && homeGoal != null && ball != null;

        if (autopilotActive)
        {
            UpdateSoccerStrategy();
            currentWr = Mathf.MoveTowards(currentWr, desiredWr, turnResponsiveness * Time.deltaTime);
            currentVr = Mathf.MoveTowards(currentVr, desiredVr, forwardResponsiveness * Time.deltaTime);
        }
        else
        {
            HandleManualInput();
        }

        if (Input.GetKey(KeyCode.Space) && ball != null)
        {
            ball.position = posb0;
        }

        if (!train || autopilotActive)
        {
            vr = currentVr;
            wr = currentWr;
        }

        if (kickCooldownTimer > 0f)
        {
            kickCooldownTimer = Mathf.Max(0f, kickCooldownTimer - Time.fixedDeltaTime);
        }

        Time.timeScale = accelerate ? 20f : 1f;

        tp++;
        int halfCycle = Mathf.Clamp(gaitHalfCycleFrames, 12, 40);
        if (tp > 0 && tp <= halfCycle)
        {
            tp0 = tp;
            uf1 = (-Mathf.Cos(Mathf.PI * 2f * tp0 / halfCycle) + 1f) / 2f;
            uf2 = 0f;
        }
        else if (tp > halfCycle && tp <= 2 * halfCycle)
        {
            tp0 = tp - halfCycle;
            uf1 = 0f;
            uf2 = (-Mathf.Cos(Mathf.PI * 2f * tp0 / halfCycle) + 1f) / 2f;
        }
        else if (tp >= 2 * halfCycle)
        {
            tp = 0;
        }

        ApplyRewards();
        CheckGoalState();
        MonitorEpisodeProgress();

        if (enableDebugDraw && ball != null && opponentGoal != null && homeGoal != null)
        {
            Debug.DrawLine(body.position, ball.position, Color.yellow);
            Debug.DrawLine(ball.position, opponentGoal.position, Color.red);
            Debug.DrawLine(ball.position, homeGoal.position, Color.blue);
        }
    }

    void HandleManualInput()
    {
        if (!wasd)
        {
            currentWr = Mathf.MoveTowards(currentWr,
                Input.GetKey(KeyCode.LeftArrow) ? -1f :
                Input.GetKey(KeyCode.RightArrow) ? 1f : 0f,
                turnResponsiveness * Time.deltaTime);

            currentVr = Mathf.MoveTowards(currentVr,
                Input.GetKey(KeyCode.UpArrow) ? chaseSpeed : 0f,
                forwardResponsiveness * Time.deltaTime);

            if (Input.GetKey(KeyCode.RightControl) && train)
            {
                EndEpisode();
            }
        }
        else
        {
            currentWr = Mathf.MoveTowards(currentWr,
                Input.GetKey(KeyCode.A) ? -1f :
                Input.GetKey(KeyCode.D) ? 1f : 0f,
                turnResponsiveness * Time.deltaTime);

            currentVr = Mathf.MoveTowards(currentVr,
                Input.GetKey(KeyCode.W) ? chaseSpeed : 0f,
                forwardResponsiveness * Time.deltaTime);

            if (Input.GetKey(KeyCode.LeftControl) && train)
            {
                EndEpisode();
            }
        }
    }

    void UpdateSoccerStrategy()
    {
        desiredVr = 0f;
        desiredWr = 0f;

        if (ball == null || opponentGoal == null || homeGoal == null)
        {
            ApplyAvoidance(Vector3.forward, Vector3.zero, body.position);
            return;
        }

        Vector3 bodyPos = body.position;
        Vector3 forwardFlat = Flatten(body.forward);
        if (forwardFlat.sqrMagnitude < 0.0001f)
        {
            forwardFlat = Vector3.forward;
        }

        Vector3 flatToBall = Flatten(ball.position - bodyPos);
        if (flatToBall.sqrMagnitude < 0.0001f)
        {
            ApplyAvoidance(forwardFlat, Vector3.zero, bodyPos);
            return;
        }

        float distToBall = flatToBall.magnitude;
        Vector3 flatToBallDir = flatToBall.normalized;
        float angleToBall = Vector3.SignedAngle(forwardFlat.normalized, flatToBallDir, Vector3.up);
        float absAngleToBall = Mathf.Abs(angleToBall);

        float moveTolerance = Mathf.Min(ballAlignmentTolerance, Mathf.Max(0.5f, moveAfterTurnTolerance));
        bool alignedWithBall = absAngleToBall <= ballAlignmentTolerance;
        bool readyToAdvance = absAngleToBall <= moveTolerance;

        if (!alignedWithBall)
        {
            desiredWr = Mathf.Clamp(angleToBall / Mathf.Max(5f, alignmentTurnGain), -1f, 1f);
            desiredVr = 0f;
            ApplyAvoidance(forwardFlat, flatToBall, bodyPos);
            return;
        }

        desiredWr = Mathf.Clamp(angleToBall / Mathf.Max(5f, headingGain), -1f, 1f);
        desiredVr = readyToAdvance ? chaseSpeed : 0f;

        bool closeToBall = distToBall <= kickPreparationDistance;

        if (closeToBall)
        {
            Vector3 toGoalFromBall = Flatten(opponentGoal.position - ball.position);
            if (toGoalFromBall.sqrMagnitude > 0.0001f)
            {
                Vector3 toGoalDir = toGoalFromBall.normalized;
                float angleToGoal = Vector3.SignedAngle(forwardFlat.normalized, toGoalDir, Vector3.up);
                float absGoalAngle = Mathf.Abs(angleToGoal);

                float goalMoveTolerance = Mathf.Min(goalAlignmentTolerance, Mathf.Max(0.5f, moveAfterTurnTolerance));
                bool alignedToGoal = absGoalAngle <= goalAlignmentTolerance;
                bool goalTurnComplete = absGoalAngle <= goalMoveTolerance;

                desiredWr = Mathf.Clamp(angleToGoal / Mathf.Max(5f, goalTurnGain), -1f, 1f);
                if (goalTurnComplete)
                {
                    float approachSpeed = distToBall > possessionDistance ? chaseSpeed : closeApproachSpeed;
                    desiredVr = approachSpeed;

                    if (alignedToGoal)
                    {
                        StartKick(flatToBall);
                    }
                }
                else
                {
                    desiredVr = 0f;
                }
            }
        }

        ApplyAvoidance(forwardFlat, flatToBall, bodyPos);
    }

    void ApplyAvoidance(Vector3 forwardFlat, Vector3 flatToBall, Vector3 bodyPos)
    {
        if (opponent != null)
        {
            Vector3 flatToOpponent = Flatten(opponent.position - bodyPos);
            float opponentDist = flatToOpponent.magnitude;

            if (opponentDist < avoidanceRadius && flatToOpponent.sqrMagnitude > 0.0001f)
            {
                float side = Mathf.Sign(Vector3.Cross(forwardFlat, flatToOpponent).y);
                desiredWr += avoidanceTurn * side;
                desiredVr = Mathf.Min(desiredVr, retreatSpeed);
            }
        }

        desiredVr = Mathf.Clamp(desiredVr, 0f, chaseSpeed);
        desiredWr = Mathf.Clamp(desiredWr, -1f, 1f);
    }

    float HeadingCommand(Vector3 forwardFlat, Vector3 targetFlat)
    {
        if (forwardFlat.sqrMagnitude < 0.0001f || targetFlat.sqrMagnitude < 0.0001f)
        {
            return 0f;
        }

        float angle = Vector3.SignedAngle(forwardFlat.normalized, targetFlat.normalized, Vector3.up);
        float gain = Mathf.Max(5f, headingGain);
        return Mathf.Clamp(angle / gain, -1f, 1f);
    }

    Vector3 ComputeInterceptionPoint(Vector3 ballPos)
    {
        if (homeGoal == null)
        {
            return ballPos;
        }

        Vector3 goalPos = homeGoal.position;
        Vector3 toBall = Flatten(ballPos - goalPos);
        if (toBall.sqrMagnitude < 0.001f)
        {
            return goalPos;
        }

        Vector3 projectedVelocity = ball != null ? Flatten(ball.velocity) * interceptVelocityScale : Vector3.zero;
        float distanceAlong = Mathf.Clamp(toBall.magnitude - interceptBuffer, 0.8f, toBall.magnitude);
        return goalPos + toBall.normalized * distanceAlong + projectedVelocity;
    }

    void StartKick(Vector3 flatToBall)
    {
        if (kickCooldownTimer > 0f || kickTimer > 0f || flatToBall.sqrMagnitude < 0.0001f)
        {
            return;
        }

        float side = Vector3.Cross(body.forward, flatToBall.normalized).y;
        activeKickFoot = side >= 0f ? KickFoot.Left : KickFoot.Right;

        kickTimer = kickDuration;
        kickCooldownTimer = kickCooldown;
    }

    void ApplyRewards()
    {
        Vector3 vel = body.InverseTransformDirection(arts[0].velocity);
        Vector3 angVel = body.InverseTransformDirection(arts[0].angularVelocity);

        float pitch = Mathf.Abs(EulerTrans(body.eulerAngles.x));
        float roll = Mathf.Abs(EulerTrans(body.eulerAngles.z));
        float balanceReward = Mathf.Clamp01(1f - (pitch + roll) / 90f) * 0.12f;
        float yawPenalty = -0.04f * Mathf.Abs(angVel.y - wr);

        float targetSpeed = train ? Mathf.Max(minTrainingSpeed, vr) : vr;
        float forwardSpeed = Mathf.Clamp(vel.z, -0.5f, targetSpeed + 1.2f);
        float speedReward = forwardSpeedRewardScale * Mathf.Max(forwardSpeed, 0f)
                            - 0.18f * Mathf.Abs(forwardSpeed - targetSpeed)
                            - lateralPenaltyScale * Mathf.Abs(vel.x);

        float facingBallReward = 0f;
        float approachReward = 0f;
        float ballGoalReward = 0f;
        float possessionBonus = 0f;

        if (ball != null)
        {
            Vector3 forwardFlat = Flatten(body.forward);
            Vector3 flatToBall = Flatten(ball.position - body.position);
            float currentBallDistance = flatToBall.magnitude;

            if (forwardFlat.sqrMagnitude > 0.0001f && flatToBall.sqrMagnitude > 0.0001f)
            {
                float dot = Vector3.Dot(forwardFlat.normalized, flatToBall.normalized);
                facingBallReward = 0.12f * dot;
            }

            if (prevBallDistance > 0f)
            {
                approachReward = Mathf.Clamp(prevBallDistance - currentBallDistance, -0.08f, 0.18f);
            }

            if (currentBallDistance < possessionDistance)
            {
                possessionBonus = 0.05f;
            }

            prevBallDistance = currentBallDistance;
        }

        if (ball != null && opponentGoal != null)
        {
            float currentBallGoalDistance = Vector3.Distance(ball.position, opponentGoal.position);
            if (prevBallGoalDistance > 0f)
            {
                ballGoalReward = Mathf.Clamp(prevBallGoalDistance - currentBallGoalDistance, -0.08f, 0.25f);
            }
            prevBallGoalDistance = currentBallGoalDistance;
        }

        float reward = balanceReward + yawPenalty + speedReward + facingBallReward + approachReward + ballGoalReward + possessionBonus;
        AddReward(reward);

        if (Mathf.Abs(EulerTrans(body.eulerAngles.x)) > 20f || Mathf.Abs(EulerTrans(body.eulerAngles.z)) > 20f)
        {
            if (train)
            {
                EndEpisode();
            }
        }
    }

    void CheckGoalState()
    {
        if (goalRegisteredThisEpisode || ball == null)
        {
            return;
        }

        if (opponentGoal != null)
        {
            float toOpponentGoal = Vector3.Distance(ball.position, opponentGoal.position);
            if (toOpponentGoal <= goalDetectionRadius)
            {
                goalRegisteredThisEpisode = true;
                OnGoalScored(true);
                return;
            }
        }

        if (homeGoal != null)
        {
            float toHomeGoal = Vector3.Distance(ball.position, homeGoal.position);
            if (toHomeGoal <= goalDetectionRadius)
            {
                goalRegisteredThisEpisode = true;
                OnGoalScored(false);
            }
        }
    }

    void MonitorEpisodeProgress()
    {
        if (!train)
        {
            stagnationStepCount = 0;
            lastBodyPosition = body.position;
            if (ball != null)
            {
                lastBallPosition = ball.position;
            }
            return;
        }

        float thresholdSqr = stagnationDistanceThreshold * stagnationDistanceThreshold;
        bool advanced = false;

        Vector3 bodyDelta = body.position - lastBodyPosition;
        if (bodyDelta.sqrMagnitude > thresholdSqr)
        {
            advanced = true;
        }

        if (ball != null)
        {
            Vector3 ballDelta = ball.position - lastBallPosition;
            if (ballDelta.sqrMagnitude > thresholdSqr)
            {
                advanced = true;
            }
            lastBallPosition = ball.position;
        }

        if (advanced)
        {
            stagnationStepCount = 0;
        }
        else
        {
            stagnationStepCount++;
            if (stagnationStepLimit > 0 && stagnationStepCount >= stagnationStepLimit)
            {
                AddReward(-0.5f);
                if (train)
                {
                    EndEpisode();
                }
                stagnationStepCount = 0;
            }
        }

        lastBodyPosition = body.position;
    }

    public void OnGoalScored(bool ourGoal)
    {
        ResetBallState();

        AddReward(ourGoal ? 3f : -3f);
        if (train)
        {
            EndEpisode();
        }
    }

    Vector3 Flatten(Vector3 v)
    {
        v.y = 0f;
        return v;
    }
}




////尝试切换模型
//using UnityEngine;
//using Unity.Mathematics;
//using Unity.Burst;
//using Unity.MLAgents;
//using Unity.MLAgents.Actuators;
//using Unity.MLAgents.Sensors;
//using Unity.MLAgents.Policies;
//using Random = UnityEngine.Random;
//using System.Collections.Generic;
//using Unity.Sentis;

//public class TinkerSoccer : Agent
//{
//    int tp = 0;
//    int tp0 = 0;

//    float uf1 = 0f;
//    float uf2 = 0f;
//    readonly float[] u = new float[12];
//    readonly float[] utotal = new float[12];

//    Transform body;

//    public Rigidbody ball;
//    public bool fixbody = false;
//    public bool train;
//    public bool accelerate;
//    public bool wasd = false;
//    public int ObservationNum;
//    public int ActionNum;

//    readonly List<float> P0 = new List<float>();
//    readonly List<float> W0 = new List<float>();
//    Vector3 pos0;
//    Vector3 posb0;
//    Quaternion rot0;
//    Quaternion rotb0;
//    ArticulationBody[] arts = new ArticulationBody[40];
//    readonly ArticulationBody[] acts = new ArticulationBody[12];

//    public float vr = 0f;
//    public float wr = 0f;
//    float currentWr = 0f;
//    float currentVr = 0f;

//    [Header("Soccer References")]
//    public Transform opponent;
//    public Transform opponentGoal;
//    public Transform homeGoal;

//    [Header("Soccer Strategy Settings")]
//    public float chaseSpeed = 0.9f;
//    public float defendSpeed = 0.65f;
//    public float retreatSpeed = 0.45f;
//    public float headingGain = 28f;
//    public float turnResponsiveness = 4.5f;
//    public float forwardResponsiveness = 1.8f;
//    public float possessionDistance = 0.6f;
//    public float kickPreparationDistance = 0.75f;
//    public float kickAlignmentTolerance = 12f;
//    public float avoidanceRadius = 1.1f;
//    public float avoidanceTurn = 0.6f;
//    public float interceptBuffer = 1.2f;
//    public float interceptVelocityScale = 0.35f;
//    public float distanceBias = 0.05f;

//    [Header("Alignment Settings")]
//    public float ballAlignmentTolerance = 6f;
//    public float goalAlignmentTolerance = 8f;
//    public float alignmentTurnGain = 18f;
//    public float goalTurnGain = 22f;
//    public float closeApproachSpeed = 0.55f;

//    [Header("Kick Settings")]
//    public float kickDuration = 0.28f;
//    public float kickCooldown = 0.9f;
//    public float kickHipAmplitude = 40f;
//    public float kickKneeAmplitude = 70f;
//    public float kickAnkleAmplitude = 35f;

//    [Header("Locomotion Tuning")]
//    public float strideOffset = 38f;
//    public float strideAmplitude = 22f;
//    public int gaitHalfCycleFrames = 18;
//    public float minTrainingSpeed = 0.45f;
//    public float forwardSpeedRewardScale = 0.25f;
//    public float lateralPenaltyScale = 0.07f;
//    public int configuredMaxStep = 2800;
//    public int stagnationStepLimit = 800;
//    public float stagnationDistanceThreshold = 0.02f;

//    [Header("Goal Detection")]
//    public float goalDetectionRadius = 0.55f;

//    [Header("Motion Control")]
//    public float moveAfterTurnTolerance = 3f;

//    [Header("Debug")]
//    public bool enableDebugDraw = false;

//    [Header("Model Switching")]
//    public ModelAsset soccerPolicyModel; // 47维输入模型
//    public ModelAsset standPolicyModel;  // 33维输入模型
//    public float fallTiltThreshold = 28f;
//    public float standTiltThreshold = 6f;
//    public float standStableMinDuration = 1f;
//    public float standStableMaxDuration = 2f;

//    // 模型类型与维度管理
//    public enum ModelType { Soccer, Stand }
//    private ModelType currentModelType;
//    private const int StandModelObservationNum = 33;
//    private const int SoccerModelObservationNum = 47;
//    private float standStableTimer = 0f;
//    private float targetStandStableTime = 0f;
//    private const string BehaviorName = "gewu";

//    enum KickFoot { None, Left, Right }
//    KickFoot activeKickFoot = KickFoot.None;

//    float desiredVr = 0f;
//    float desiredWr = 0f;
//    float kickTimer = 0f;
//    float kickCooldownTimer = 0f;
//    float prevBallDistance = 0f;
//    float prevBallGoalDistance = 0f;
//    ArticulationBody opponentBody;
//    bool _isClone = false;
//    bool goalRegisteredThisEpisode = false;

//    int stagnationStepCount = 0;
//    Vector3 lastBallPosition = Vector3.zero;
//    Vector3 lastBodyPosition = Vector3.zero;

//    public override void Initialize()
//    {
//        arts = GetComponentsInChildren<ArticulationBody>();
//        ActionNum = 0;
//        for (int k = 0; k < arts.Length; k++)
//        {
//            if (arts[k].jointType == ArticulationJointType.RevoluteJoint && ActionNum < acts.Length)
//            {
//                acts[ActionNum] = arts[k];
//                ActionNum++;
//            }
//        }

//        body = arts[0].transform;
//        pos0 = body.position;
//        rot0 = body.rotation;
//        if (ball != null)
//        {
//            posb0 = ball.position;
//            rotb0 = ball.rotation;
//            lastBallPosition = ball.position;
//        }

//        arts[0].GetJointPositions(P0);
//        arts[0].GetJointVelocities(W0);
//        accelerate = train;

//        // 初始化模型
//        currentModelType = ModelType.Soccer;
//        if (soccerPolicyModel != null)
//        {
//            SetModel(BehaviorName, soccerPolicyModel);
//        }

//        CacheOpponentBody();

//        if (configuredMaxStep > 0)
//        {
//            MaxStep = configuredMaxStep;
//        }

//        lastBodyPosition = body.position;
//    }

//    void CacheOpponentBody()
//    {
//        if (opponent == null)
//        {
//            opponentBody = null;
//            return;
//        }

//        opponentBody = opponent.GetComponentInChildren<ArticulationBody>();
//        if (opponentBody == null)
//        {
//            opponentBody = opponent.GetComponent<ArticulationBody>();
//        }
//    }

//    void Start()
//    {
//        Time.fixedDeltaTime = 0.01f;

//        if (train && !_isClone)
//        {
//            for (int i = 1; i < 4; i++)
//            {
//                GameObject clone = Instantiate(gameObject);
//                clone.name = $"{name}_Clone_{i}";
//                var controller = clone.GetComponent<TinkerSoccer>();
//                controller._isClone = true;
//                controller.train = true;
//                controller.accelerate = accelerate;
//            }
//        }
//    }

//    public override void OnEpisodeBegin()
//    {
//        tp = 0;
//        kickTimer = 0f;
//        kickCooldownTimer = 0f;
//        activeKickFoot = KickFoot.None;
//        stagnationStepCount = 0;
//        goalRegisteredThisEpisode = false;
//        standStableTimer = 0f;
//        targetStandStableTime = 0f;

//        // 重置为踢球模型
//        currentModelType = ModelType.Soccer;
//        if (soccerPolicyModel != null)
//        {
//            SetModel(BehaviorName, soccerPolicyModel);
//        }

//        for (int i = 0; i < u.Length; i++)
//        {
//            u[i] = 0f;
//        }

//        ObservationNum = SoccerModelObservationNum;

//        if (fixbody)
//        {
//            arts[0].immovable = true;
//        }
//        else
//        {
//            arts[0].TeleportRoot(pos0, rot0);
//            arts[0].velocity = Vector3.zero;
//            arts[0].angularVelocity = Vector3.zero;
//            arts[0].SetJointPositions(P0);
//            arts[0].SetJointVelocities(W0);
//        }

//        ResetBallState();

//        vr = 0f;
//        wr = 0f;
//        currentVr = 0f;
//        currentWr = 0f;

//        lastBodyPosition = body.position;

//        if (train)
//        {
//            float minSpeed = Mathf.Min(chaseSpeed, minTrainingSpeed);
//            float maxSpeed = Mathf.Max(chaseSpeed, minTrainingSpeed + 0.25f);
//            vr = Random.Range(minSpeed, maxSpeed);
//            wr = Random.Range(-1f, 1f);
//        }
//    }

//    void ResetBallState()
//    {
//        if (ball == null)
//        {
//            prevBallDistance = 0f;
//            prevBallGoalDistance = 0f;
//            lastBallPosition = Vector3.zero;
//            return;
//        }

//        ball.velocity = Vector3.zero;
//        ball.angularVelocity = Vector3.zero;
//        ball.position = posb0;
//        ball.rotation = rotb0;

//        prevBallDistance = Vector3.Distance(body.position, ball.position);
//        prevBallGoalDistance = opponentGoal != null ? Vector3.Distance(ball.position, opponentGoal.position) : 0f;
//        lastBallPosition = ball.position;
//    }

//    public override void CollectObservations(VectorSensor sensor)
//    {
//        // 基础观测（33维）
//        sensor.AddObservation(body.InverseTransformDirection(Vector3.down)); // 重力方向（3）
//        sensor.AddObservation(body.InverseTransformDirection(arts[0].angularVelocity)); // 角速度（3）
//        sensor.AddObservation(body.InverseTransformDirection(arts[0].velocity)); // 线速度（3）

//        // 关节状态（24维：12个关节×位置+速度）
//        for (int i = 0; i < ActionNum; i++)
//        {
//            sensor.AddObservation(acts[i].jointPosition[0]);
//            sensor.AddObservation(acts[i].jointVelocity[0]);
//        }

//        // 踢球模型额外观测（14维）
//        if (currentModelType == ModelType.Soccer)
//        {
//            Vector3 ballPosLocal = ball != null ? body.InverseTransformPoint(ball.position) : Vector3.zero;
//            Vector3 opponentPosLocal = opponent != null ? body.InverseTransformPoint(opponent.position) : Vector3.zero;
//            Vector3 opponentGoalLocal = opponentGoal != null ? body.InverseTransformPoint(opponentGoal.position) : Vector3.zero;
//            Vector3 homeGoalLocal = homeGoal != null ? body.InverseTransformPoint(homeGoal.position) : Vector3.zero;

//            sensor.AddObservation(ballPosLocal); // 3
//            sensor.AddObservation(opponentPosLocal); // 3
//            sensor.AddObservation(opponentGoalLocal); // 3
//            sensor.AddObservation(homeGoalLocal); // 3
//            sensor.AddObservation(vr); // 1
//            sensor.AddObservation(wr); // 1
//        }
//    }

//    float EulerTrans(float eulerAngle)
//    {
//        return eulerAngle <= 180f ? eulerAngle : eulerAngle - 360f;
//    }

//    public override void OnActionReceived(ActionBuffers actionBuffers)
//    {
//        for (int i = 0; i < utotal.Length; i++)
//        {
//            utotal[i] = 0f;
//        }

//        var continuousActions = actionBuffers.ContinuousActions;
//        const float kk = 0.9f;
//        float[] kb = new float[12] { 12f, 12f, 34f, 56f, 34f, 12f, 12f, 34f, 56f, 34f, 0f, 0f };

//        for (int i = 0; i < ActionNum; i++)
//        {
//            u[i] = u[i] * kk + (1f - kk) * continuousActions[i];
//            utotal[i] = kb[i] * u[i];
//            if (fixbody)
//            {
//                utotal[i] = 0f;
//            }
//        }

//        float d0 = strideOffset;
//        float dh = strideAmplitude;
//        int[] idx = new int[6] { -2, -3, 4, 7, 8, -9 };

//        if (ActionNum > 9)
//        {
//            utotal[Mathf.Abs(idx[0])] += (dh * uf1 + d0) * Mathf.Sign(idx[0]);
//            utotal[Mathf.Abs(idx[1])] -= 2f * (dh * uf1 + d0) * Mathf.Sign(idx[1]);
//            utotal[Mathf.Abs(idx[2])] += (dh * uf1 + d0) * Mathf.Sign(idx[2]);
//            utotal[Mathf.Abs(idx[3])] += (dh * uf2 + d0) * Mathf.Sign(idx[3]);
//            utotal[Mathf.Abs(idx[4])] -= 2f * (dh * uf2 + d0) * Mathf.Sign(idx[4]);
//            utotal[Mathf.Abs(idx[5])] += (dh * uf2 + d0) * Mathf.Sign(idx[5]);
//        }

//        ApplyKickOffsets();

//        for (int i = 0; i < ActionNum; i++)
//        {
//            SetJointTargetDeg(acts[i], utotal[i]);
//        }
//    }

//    void ApplyKickOffsets()
//    {
//        if (kickTimer <= 0f || ActionNum < 10)
//        {
//            return;
//        }

//        float normalized = 1f - Mathf.Clamp01(kickTimer / Mathf.Max(0.001f, kickDuration));
//        float swing = Mathf.Sin(normalized * Mathf.PI);

//        int hipIndex = activeKickFoot == KickFoot.Left ? 2 : 7;
//        int kneeIndex = activeKickFoot == KickFoot.Left ? 3 : 8;
//        int ankleIndex = activeKickFoot == KickFoot.Left ? 4 : 9;

//        if (hipIndex >= 0 && hipIndex < utotal.Length)
//        {
//            utotal[hipIndex] += kickHipAmplitude * swing;
//        }
//        if (kneeIndex >= 0 && kneeIndex < utotal.Length)
//        {
//            utotal[kneeIndex] -= kickKneeAmplitude * swing;
//        }
//        if (ankleIndex >= 0 && ankleIndex < utotal.Length)
//        {
//            utotal[ankleIndex] += kickAnkleAmplitude * swing;
//        }

//        kickTimer = Mathf.Max(0f, kickTimer - Time.fixedDeltaTime);
//        if (kickTimer <= 0f)
//        {
//            activeKickFoot = KickFoot.None;
//        }
//    }

//    void SetJointTargetDeg(ArticulationBody joint, float x)
//    {
//        var drive = joint.xDrive;
//        drive.stiffness = 2200f;
//        drive.damping = 110f;
//        drive.forceLimit = 380f;
//        drive.target = x;
//        joint.xDrive = drive;
//    }

//    public override void Heuristic(in ActionBuffers actionsOut)
//    {
//        var continuousActions = actionsOut.ContinuousActions;
//        for (int i = 0; i < continuousActions.Length; i++)
//        {
//            continuousActions[i] = 0;
//        }
//    }

//    void Update()
//    {
//        // 主线程处理模型切换
//        HandleModelSwitching();
//        Time.timeScale = accelerate && !train ? 20f : 1f;
//    }

//    void FixedUpdate()
//    {
//        if (opponentBody == null)
//        {
//            CacheOpponentBody();
//        }

//        bool autopilotActive = !train && opponent != null && opponentGoal != null && homeGoal != null && ball != null;

//        if (autopilotActive)
//        {
//            UpdateSoccerStrategy();
//            currentWr = Mathf.MoveTowards(currentWr, desiredWr, turnResponsiveness * Time.deltaTime);
//            currentVr = Mathf.MoveTowards(currentVr, desiredVr, forwardResponsiveness * Time.deltaTime);
//        }
//        else
//        {
//            HandleManualInput();
//        }

//        if (Input.GetKey(KeyCode.Space) && ball != null)
//        {
//            ball.position = posb0;
//        }

//        if (!train || autopilotActive)
//        {
//            vr = currentVr;
//            wr = currentWr;
//        }

//        if (kickCooldownTimer > 0f)
//        {
//            kickCooldownTimer = Mathf.Max(0f, kickCooldownTimer - Time.fixedDeltaTime);
//        }

//        tp++;
//        int halfCycle = Mathf.Clamp(gaitHalfCycleFrames, 12, 40);
//        if (tp > 0 && tp <= halfCycle)
//        {
//            tp0 = tp;
//            uf1 = (-Mathf.Cos(Mathf.PI * 2f * tp0 / halfCycle) + 1f) / 2f;
//            uf2 = 0f;
//        }
//        else if (tp > halfCycle && tp <= 2 * halfCycle)
//        {
//            tp0 = tp - halfCycle;
//            uf1 = 0f;
//            uf2 = (-Mathf.Cos(Mathf.PI * 2f * tp0 / halfCycle) + 1f) / 2f;
//        }
//        else if (tp >= 2 * halfCycle)
//        {
//            tp = 0;
//        }

//        ApplyRewards();
//        CheckGoalState();
//        MonitorEpisodeProgress();

//        if (enableDebugDraw && ball != null && opponentGoal != null && homeGoal != null)
//        {
//            Debug.DrawLine(body.position, ball.position, Color.yellow);
//            Debug.DrawLine(ball.position, opponentGoal.position, Color.red);
//            Debug.DrawLine(ball.position, homeGoal.position, Color.blue);
//        }
//    }

//    void HandleManualInput()
//    {
//        if (!wasd)
//        {
//            currentWr = Mathf.MoveTowards(currentWr,
//                Input.GetKey(KeyCode.LeftArrow) ? -1f :
//                Input.GetKey(KeyCode.RightArrow) ? 1f : 0f,
//                turnResponsiveness * Time.deltaTime);

//            currentVr = Mathf.MoveTowards(currentVr,
//                Input.GetKey(KeyCode.UpArrow) ? chaseSpeed : 0f,
//                forwardResponsiveness * Time.deltaTime);

//            if (Input.GetKey(KeyCode.RightControl) && train)
//            {
//                EndEpisode();
//            }
//        }
//        else
//        {
//            currentWr = Mathf.MoveTowards(currentWr,
//                Input.GetKey(KeyCode.A) ? -1f :
//                Input.GetKey(KeyCode.D) ? 1f : 0f,
//                turnResponsiveness * Time.deltaTime);

//            currentVr = Mathf.MoveTowards(currentVr,
//                Input.GetKey(KeyCode.W) ? chaseSpeed : 0f,
//                forwardResponsiveness * Time.deltaTime);

//            if (Input.GetKey(KeyCode.LeftControl) && train)
//            {
//                EndEpisode();
//            }
//        }
//    }

//    void UpdateSoccerStrategy()
//    {
//        desiredVr = 0f;
//        desiredWr = 0f;

//        if (ball == null || opponentGoal == null || homeGoal == null)
//        {
//            ApplyAvoidance(Vector3.forward, Vector3.zero, body.position);
//            return;
//        }

//        Vector3 bodyPos = body.position;
//        Vector3 forwardFlat = Flatten(body.forward);
//        if (forwardFlat.sqrMagnitude < 0.0001f)
//        {
//            forwardFlat = Vector3.forward;
//        }

//        Vector3 flatToBall = Flatten(ball.position - bodyPos);
//        if (flatToBall.sqrMagnitude < 0.0001f)
//        {
//            ApplyAvoidance(forwardFlat, Vector3.zero, bodyPos);
//            return;
//        }

//        float distToBall = flatToBall.magnitude;
//        Vector3 flatToBallDir = flatToBall.normalized;
//        float angleToBall = Vector3.SignedAngle(forwardFlat.normalized, flatToBallDir, Vector3.up);
//        float absAngleToBall = Mathf.Abs(angleToBall);

//        float moveTolerance = Mathf.Min(ballAlignmentTolerance, Mathf.Max(0.5f, moveAfterTurnTolerance));
//        bool alignedWithBall = absAngleToBall <= ballAlignmentTolerance;
//        bool readyToAdvance = absAngleToBall <= moveTolerance;

//        if (!alignedWithBall)
//        {
//            desiredWr = Mathf.Clamp(angleToBall / Mathf.Max(5f, alignmentTurnGain), -1f, 1f);
//            desiredVr = 0f;
//            ApplyAvoidance(forwardFlat, flatToBall, bodyPos);
//            return;
//        }

//        desiredWr = Mathf.Clamp(angleToBall / Mathf.Max(5f, headingGain), -1f, 1f);
//        desiredVr = readyToAdvance ? chaseSpeed : 0f;

//        bool closeToBall = distToBall <= kickPreparationDistance;

//        if (closeToBall)
//        {
//            Vector3 toGoalFromBall = Flatten(opponentGoal.position - ball.position);
//            if (toGoalFromBall.sqrMagnitude > 0.0001f)
//            {
//                Vector3 toGoalDir = toGoalFromBall.normalized;
//                float angleToGoal = Vector3.SignedAngle(forwardFlat.normalized, toGoalDir, Vector3.up);
//                float absGoalAngle = Mathf.Abs(angleToGoal);

//                float goalMoveTolerance = Mathf.Min(goalAlignmentTolerance, Mathf.Max(0.5f, moveAfterTurnTolerance));
//                bool alignedToGoal = absGoalAngle <= goalAlignmentTolerance;
//                bool goalTurnComplete = absGoalAngle <= goalMoveTolerance;

//                desiredWr = Mathf.Clamp(angleToGoal / Mathf.Max(5f, goalTurnGain), -1f, 1f);
//                if (goalTurnComplete)
//                {
//                    float approachSpeed = distToBall > possessionDistance ? chaseSpeed : closeApproachSpeed;
//                    desiredVr = approachSpeed;

//                    if (alignedToGoal)
//                    {
//                        StartKick(flatToBall);
//                    }
//                }
//                else
//                {
//                    desiredVr = 0f;
//                }
//            }
//        }

//        ApplyAvoidance(forwardFlat, flatToBall, bodyPos);
//    }

//    void ApplyAvoidance(Vector3 forwardFlat, Vector3 flatToBall, Vector3 bodyPos)
//    {
//        if (opponent != null)
//        {
//            Vector3 flatToOpponent = Flatten(opponent.position - bodyPos);
//            float opponentDist = flatToOpponent.magnitude;

//            if (opponentDist < avoidanceRadius && flatToOpponent.sqrMagnitude > 0.0001f)
//            {
//                float side = Mathf.Sign(Vector3.Cross(forwardFlat, flatToOpponent).y);
//                desiredWr += avoidanceTurn * side;
//                desiredVr = Mathf.Min(desiredVr, retreatSpeed);
//            }
//        }

//        desiredVr = Mathf.Clamp(desiredVr, 0f, chaseSpeed);
//        desiredWr = Mathf.Clamp(desiredWr, -1f, 1f);
//    }

//    float HeadingCommand(Vector3 forwardFlat, Vector3 targetFlat)
//    {
//        if (forwardFlat.sqrMagnitude < 0.0001f || targetFlat.sqrMagnitude < 0.0001f)
//        {
//            return 0f;
//        }

//        float angle = Vector3.SignedAngle(forwardFlat.normalized, targetFlat.normalized, Vector3.up);
//        float gain = Mathf.Max(5f, headingGain);
//        return Mathf.Clamp(angle / gain, -1f, 1f);
//    }

//    Vector3 ComputeInterceptionPoint(Vector3 ballPos)
//    {
//        if (homeGoal == null)
//        {
//            return ballPos;
//        }

//        Vector3 goalPos = homeGoal.position;
//        Vector3 toBall = Flatten(ballPos - goalPos);
//        if (toBall.sqrMagnitude < 0.001f)
//        {
//            return goalPos;
//        }

//        Vector3 projectedVelocity = ball != null ? Flatten(ball.velocity) * interceptVelocityScale : Vector3.zero;
//        float distanceAlong = Mathf.Clamp(toBall.magnitude - interceptBuffer, 0.8f, toBall.magnitude);
//        return goalPos + toBall.normalized * distanceAlong + projectedVelocity;
//    }

//    void StartKick(Vector3 flatToBall)
//    {
//        if (kickCooldownTimer > 0f || kickTimer > 0f || flatToBall.sqrMagnitude < 0.0001f)
//        {
//            return;
//        }

//        float side = Vector3.Cross(body.forward, flatToBall.normalized).y;
//        activeKickFoot = side >= 0f ? KickFoot.Left : KickFoot.Right;

//        kickTimer = kickDuration;
//        kickCooldownTimer = kickCooldown;
//    }

//    void ApplyRewards()
//    {
//        Vector3 vel = body.InverseTransformDirection(arts[0].velocity);
//        Vector3 angVel = body.InverseTransformDirection(arts[0].angularVelocity);

//        float pitch = Mathf.Abs(EulerTrans(body.eulerAngles.x));
//        float roll = Mathf.Abs(EulerTrans(body.eulerAngles.z));
//        float balanceReward = Mathf.Clamp01(1f - (pitch + roll) / 90f) * 0.12f;
//        float yawPenalty = -0.04f * Mathf.Abs(angVel.y - wr);

//        float targetSpeed = train ? Mathf.Max(minTrainingSpeed, vr) : vr;
//        float forwardSpeed = Mathf.Clamp(vel.z, -0.5f, targetSpeed + 1.2f);
//        float speedReward = forwardSpeedRewardScale * Mathf.Max(forwardSpeed, 0f)
//                            - 0.18f * Mathf.Abs(forwardSpeed - targetSpeed)
//                            - lateralPenaltyScale * Mathf.Abs(vel.x);

//        float facingBallReward = 0f;
//        float approachReward = 0f;
//        float ballGoalReward = 0f;
//        float possessionBonus = 0f;

//        // 只有踢球模型才计算足球相关奖励
//        if (currentModelType == ModelType.Soccer && ball != null)
//        {
//            Vector3 forwardFlat = Flatten(body.forward);
//            Vector3 flatToBall = Flatten(ball.position - body.position);
//            float currentBallDistance = flatToBall.magnitude;

//            if (forwardFlat.sqrMagnitude > 0.0001f && flatToBall.sqrMagnitude > 0.0001f)
//            {
//                float dot = Vector3.Dot(forwardFlat.normalized, flatToBall.normalized);
//                facingBallReward = 0.12f * dot;
//            }

//            if (prevBallDistance > 0f)
//            {
//                approachReward = Mathf.Clamp(prevBallDistance - currentBallDistance, -0.08f, 0.18f);
//            }

//            if (currentBallDistance < possessionDistance)
//            {
//                possessionBonus = 0.05f;
//            }

//            prevBallDistance = currentBallDistance;
//        }

//        if (currentModelType == ModelType.Soccer && ball != null && opponentGoal != null)
//        {
//            float currentBallGoalDistance = Vector3.Distance(ball.position, opponentGoal.position);
//            if (prevBallGoalDistance > 0f)
//            {
//                ballGoalReward = Mathf.Clamp(prevBallGoalDistance - currentBallGoalDistance, -0.08f, 0.25f);
//            }
//            prevBallGoalDistance = currentBallGoalDistance;
//        }

//        float reward = balanceReward + yawPenalty + speedReward + facingBallReward + approachReward + ballGoalReward + possessionBonus;
//        AddReward(reward);

//        if (train)
//        {
//            bool fell = Mathf.Abs(EulerTrans(body.eulerAngles.x)) > fallTiltThreshold
//                        || Mathf.Abs(EulerTrans(body.eulerAngles.z)) > fallTiltThreshold;
//            if (fell)
//            {
//                EndEpisode();
//            }
//        }
//    }

//    void CheckGoalState()
//    {
//        if (goalRegisteredThisEpisode || ball == null || currentModelType != ModelType.Soccer)
//        {
//            return;
//        }

//        if (opponentGoal != null)
//        {
//            float toOpponentGoal = Vector3.Distance(ball.position, opponentGoal.position);
//            if (toOpponentGoal <= goalDetectionRadius)
//            {
//                goalRegisteredThisEpisode = true;
//                OnGoalScored(true);
//                return;
//            }
//        }

//        if (homeGoal != null)
//        {
//            float toHomeGoal = Vector3.Distance(ball.position, homeGoal.position);
//            if (toHomeGoal <= goalDetectionRadius)
//            {
//                goalRegisteredThisEpisode = true;
//                OnGoalScored(false);
//            }
//        }
//    }

//    void MonitorEpisodeProgress()
//    {
//        if (!train)
//        {
//            stagnationStepCount = 0;
//            lastBodyPosition = body.position;
//            if (ball != null)
//            {
//                lastBallPosition = ball.position;
//            }
//            return;
//        }

//        float thresholdSqr = stagnationDistanceThreshold * stagnationDistanceThreshold;
//        bool advanced = false;

//        Vector3 bodyDelta = body.position - lastBodyPosition;
//        if (bodyDelta.sqrMagnitude > thresholdSqr)
//        {
//            advanced = true;
//        }

//        if (ball != null)
//        {
//            Vector3 ballDelta = ball.position - lastBallPosition;
//            if (ballDelta.sqrMagnitude > thresholdSqr)
//            {
//                advanced = true;
//            }
//            lastBallPosition = ball.position;
//        }

//        if (advanced)
//        {
//            stagnationStepCount = 0;
//        }
//        else
//        {
//            stagnationStepCount++;
//            if (stagnationStepLimit > 0 && stagnationStepCount >= stagnationStepLimit)
//            {
//                AddReward(-0.5f);
//                if (train)
//                {
//                    EndEpisode();
//                }
//                stagnationStepCount = 0;
//            }
//        }

//        lastBodyPosition = body.position;
//    }

//    public void OnGoalScored(bool ourGoal)
//    {
//        ResetBallState();

//        AddReward(ourGoal ? 15f : -15f);
//        if (train)
//        {
//            EndEpisode();
//        }
//    }

//    // 模型切换核心逻辑
//    void HandleModelSwitching()
//    {
//        if (soccerPolicyModel == null || standPolicyModel == null || arts.Length == 0)
//        {
//            return;
//        }

//        float pitch = Mathf.Abs(EulerTrans(body.eulerAngles.x));
//        float roll = Mathf.Abs(EulerTrans(body.eulerAngles.z));
//        bool fallen = pitch > fallTiltThreshold || roll > fallTiltThreshold;
//        bool upright = pitch < standTiltThreshold && roll < standTiltThreshold;

//        // 跌倒时切换到站立模型
//        if (fallen && currentModelType != ModelType.Stand)
//        {
//            SwitchToModel(ModelType.Stand);
//            return;
//        }

//        // 站立稳定后切换回踢球模型
//        if (currentModelType == ModelType.Stand)
//        {
//            if (upright)
//            {
//                if (targetStandStableTime <= 0f)
//                {
//                    targetStandStableTime = Random.Range(standStableMinDuration, standStableMaxDuration);
//                }
//                standStableTimer += Time.deltaTime;
//                if (standStableTimer >= targetStandStableTime)
//                {
//                    SwitchToModel(ModelType.Soccer);
//                }
//            }
//            else
//            {
//                standStableTimer = 0f;
//            }
//        }
//        else
//        {
//            standStableTimer = 0f;
//            targetStandStableTime = 0f;
//        }
//    }

//    // 安全切换模型的方法（修正核心错误）
//    private void SwitchToModel(ModelType targetType)
//    {
//        try
//        {
//            // 修正：使用Agent组件的enabled属性控制推理
//            this.enabled = false; // 禁用当前Agent推理

//            // 重置关节状态
//            ResetJointsForModel(targetType);

//            // 清空动作缓存
//            for (int i = 0; i < u.Length; i++)
//                u[i] = 0f;

//            // 切换模型
//            currentModelType = targetType;
//            if (targetType == ModelType.Soccer)
//            {
//                SetModel(BehaviorName, soccerPolicyModel);
//                ObservationNum = SoccerModelObservationNum;
//            }
//            else
//            {
//                SetModel(BehaviorName, standPolicyModel);
//                ObservationNum = StandModelObservationNum;
//            }

//            // 修正：重新启用Agent推理
//            this.enabled = true;

//            Debug.Log($"模型切换完成: {targetType}, 观测维度: {ObservationNum}");
//        }
//        catch (System.Exception e)
//        {
//            Debug.LogError($"模型切换失败: {e.Message}\n{e.StackTrace}");
//            // 异常时确保Agent启用，避免程序卡死
//            this.enabled = true;
//        }
//    }

//    // 根据目标模型重置关节状态
//    private void ResetJointsForModel(ModelType targetType)
//    {
//        if (targetType == ModelType.Stand)
//        {
//            // 站立模型初始关节角度（根据实际调整）
//            float[] standJoints = new float[12] { 0, 0, -5, 20, -15, 0, 0, -5, 20, -15, 0, 0 };
//            for (int i = 0; i < ActionNum; i++)
//            {
//                if (acts[i] != null)
//                {
//                    SetJointTargetDeg(acts[i], standJoints[i]);
//                    acts[i].velocity = Vector3.zero;
//                    acts[i].angularVelocity = Vector3.zero;
//                }
//            }
//        }
//        else
//        {
//            // 踢球模型仅重置速度
//            for (int i = 0; i < ActionNum; i++)
//            {
//                if (acts[i] != null)
//                {
//                    acts[i].velocity = Vector3.zero;
//                    acts[i].angularVelocity = Vector3.zero;
//                }
//            }
//        }
//    }

//    Vector3 Flatten(Vector3 v)
//    {
//        v.y = 0f;
//        return v;
//    }
//}




















































